﻿#ifndef DRAGCTRL_H
#define DRAGCTRL_H

#include <math.h>
#include <boost/signals2.hpp>
#include <boost/numeric/ublas/matrix.hpp>

/*如果你们的机器人末端有力传感器，可以检测到拖拽力，那么以下拖拽控制适用
 * 这里用最简单的实现方法，不考虑加速度和角速度和阻尼。只用虚拟合外力对速度进行映射
 * 采用指数函数曲线将拖拽力与末端速度进行映射：KV*(1-pow(1.5,FfMAX-F))
 */
using namespace boost;

namespace DRAGCTRL {//namespace DRAGCTRL start


enum {
    SAMPLEIN=100,//100ms采样间隔
    M=1,//末端虚拟质量1kg
    KV=20,//映射系数（放大倍数）,也是最大速度 （映射曲线渐近线）
    FfMAX=2,//虚拟最大静摩擦
    FMAX=30,//力的阈值，超出则停止机器人运动

};
typedef double rotMatrix[3][3];

rotMatrix rotM;

bool getRotMatrix(void *rotMatClass)
{
    if(rotMatClass==NULL){
        return false;
    }else{
        //TODO: 获取机器人末端执行机构（力传感器）坐标系到机器人基座标系的旋转矩阵，保证六维力的方向始终与与机器人基座标系重合
        memset(rotM,0,sizeof(rotM));

        return true;//注意
    }
}




class DragCtrl
{
public:
    explicit DragCtrl(void* _sourceOfRotMatrix=NULL);
    ~DragCtrl();

    bool coordinatesTrans(float *F);
    void dragCal(float* F);//传入末端传感器检测到的拖拽力假设是六维力对应x,y,z,Tx,Ty,Tz
    bool setZeroOffSet(double* F);//设置拖拽力零点
    bool calZeroOffSet(float *FVal);
    bool getActuatorVel(double *actVel);

    signals2::signal<void()> sigForceOver;//超出力的阈值，则停止机器人运动 信号
    signals2::signal<void(double* )> sigActutorVel;//经过拖拽计算传出机器人末端速度

    double getPowerBase() const;
    void setPowerBase(double value);

    void *getSourceOfRotMatrix() const;
    void setSourceOfRotMatrix(void *value);

private:
    double actuatorVel[6];//末端执行机构的速度
    double forceOffSet[6];
    double powerBase;
    double sig(double val);
    void *sourceOfRotMatrix;


};



}//namespace DRAGCTRL stop
#endif // DRAGCTRL_H
